#include <Math/Vector2.hpp>
#include <Math/Vector3.hpp>
#include <Math/Vector4.hpp>
#include <Graphics/aabb.hpp>

namespace zzz {
template<typename T>
class BoundingBox3 {
public:
  BoundingBox3()
      : ori_boundary_good_(false) {
    rot_.Identical();
    rot_back_.Identical();
  }

  BoundingBox3(const Matrix<3, 3, T> &rot) 
      : rot_(rot), 
        ori_boundary_good_(false) {
    rot_back_ = rot_.Inverted();
  }

  void SetRot(const Matrix<3, 3, T> &rot) {
    rot_ = rot;
    rot_back_ = rot_.Inverted();
    ori_boundary_good_ = false;
  }

  void Reset() {
    aabb_.Reset();
    ori_boundary_good_ = false;
  }

  void AddData(const Vector<3,T> &p) {
    aabb_.AddData(rot_ * p);
    ori_boundary_good_ = false;
  }

  template<typename T1>
  void AddData(const T1 &_begin, const T1 &_end) {
    for (T1 x=_begin; x!=_end; x++)
      AddData(*x);
  }

  void operator+=(const Vector<3,T> &p) {
    AddData(p);
  }

  void operator+=(const std::vector<Vector<3,T> > &p) {
    AddData(p.begin(), p.end());
  }

  bool operator==(const BoundingBox3<T> &box) const {
    return rot_==box.rot_ && aabb_==box.aabb_;
  }

  bool IsInside(const Vector<3,T> &pos) const {
    return aabb_.IsInside(rot_ * pos);
  }

  Vector<3, T> Center() {
    return rot_back_ * aabb_.Center();
  }

  static const int L = 0;
  static const int M = 1;
  static const int R = 2;

  Vector3i RelativePos(const Vector<3, T>& p) const {
    Vector<3, T> relp = Rotate(p);
    Vector3i pos;
    for (zuint i = 0; i < 3; ++i) {
      if (relp[i] < aabb_.Min(i)) pos[i] = L;
      else if (relp[i] > aabb_.Max(i)) pos[i] = R;
      else pos[i] = M;
    }
    return pos;
  }

  static int ToIndex(const Vector3i& pos) {
    return FastDot(pos, Vector3i(1, 3, 9));
  }
  
  static bool IsSpace(const Vector3i& pos) {
    if (ToIndex(pos) == ToIndex(Vector3i(M, M, M)))
      return true;
    return false;
  }
  static bool IsPoint(const Vector3i& pos) {
    int n = ToIndex(pos);
    if (n == ToIndex(Vector3i(L, L, L)) ||
        n == ToIndex(Vector3i(R, L, L)) ||
        n == ToIndex(Vector3i(L, R, L)) ||
        n == ToIndex(Vector3i(R, R, L)) ||
        n == ToIndex(Vector3i(L, L, R)) ||
        n == ToIndex(Vector3i(R, L, R)) ||
        n == ToIndex(Vector3i(L, R, R)) ||
        n == ToIndex(Vector3i(R, R, R)))
      return true;
    return false;
  }

  Vector<3, T> GetBoundaryPoint(const Vector3i& pos) const {
    if (!ori_boundary_good_)
      CalculateOriBoundary();
    int n = ToIndex(pos);
    if (n == ToIndex(Vector3i(L, L, L))) return ori_boundary_[0][0][0];
    if (n == ToIndex(Vector3i(R, L, L))) return ori_boundary_[1][0][0];
    if (n == ToIndex(Vector3i(L, R, L))) return ori_boundary_[0][1][0];
    if (n == ToIndex(Vector3i(R, R, L))) return ori_boundary_[1][1][0];
    if (n == ToIndex(Vector3i(L, L, R))) return ori_boundary_[0][0][1];
    if (n == ToIndex(Vector3i(R, L, R))) return ori_boundary_[1][0][1];
    if (n == ToIndex(Vector3i(L, R, R))) return ori_boundary_[0][1][1];
    if (n == ToIndex(Vector3i(R, R, R))) return ori_boundary_[1][1][1];
    ZLOGF << "This coordinate is not a point" << ZVAR(pos);
    return Vector<3, T>();
  }

  static bool IsLine(const Vector3i& pos) {
    int n = ToIndex(pos);
    if (n == ToIndex(Vector3i(M, L, L)) ||
        n == ToIndex(Vector3i(M, L, R)) ||
        n == ToIndex(Vector3i(M, R, R)) ||
        n == ToIndex(Vector3i(M, R, L)) ||
        n == ToIndex(Vector3i(L, M, L)) ||
        n == ToIndex(Vector3i(R, M, R)) ||
        n == ToIndex(Vector3i(R, M, L)) ||
        n == ToIndex(Vector3i(L, M, R)) ||
        n == ToIndex(Vector3i(L, L, M)) ||
        n == ToIndex(Vector3i(R, L, M)) ||
        n == ToIndex(Vector3i(R, R, M)) ||
        n == ToIndex(Vector3i(L, R, M)))
      return true;
    return false;
  }

  Vector<2, Vector<3, T> > GetBoundaryLine(const Vector3i& pos) const {
    if (!ori_boundary_good_)
      CalculateOriBoundary();
    int n = ToIndex(pos);
    if (n == ToIndex(Vector3i(M, L, L))) return Vector<2, Vector<3, T> >(ori_boundary_[0][0][0], ori_boundary_[1][0][0]);
    if (n == ToIndex(Vector3i(M, L, R))) return Vector<2, Vector<3, T> >(ori_boundary_[0][0][1], ori_boundary_[1][0][1]);
    if (n == ToIndex(Vector3i(M, R, R))) return Vector<2, Vector<3, T> >(ori_boundary_[0][1][1], ori_boundary_[1][1][1]);
    if (n == ToIndex(Vector3i(M, R, L))) return Vector<2, Vector<3, T> >(ori_boundary_[0][1][0], ori_boundary_[1][1][0]);
    if (n == ToIndex(Vector3i(L, M, L))) return Vector<2, Vector<3, T> >(ori_boundary_[0][0][0], ori_boundary_[0][1][0]);
    if (n == ToIndex(Vector3i(R, M, L))) return Vector<2, Vector<3, T> >(ori_boundary_[1][0][0], ori_boundary_[1][1][0]);
    if (n == ToIndex(Vector3i(R, M, R))) return Vector<2, Vector<3, T> >(ori_boundary_[1][0][1], ori_boundary_[1][1][1]);
    if (n == ToIndex(Vector3i(L, M, R))) return Vector<2, Vector<3, T> >(ori_boundary_[0][0][1], ori_boundary_[0][1][1]);
    if (n == ToIndex(Vector3i(L, L, M))) return Vector<2, Vector<3, T> >(ori_boundary_[0][0][0], ori_boundary_[0][0][1]);
    if (n == ToIndex(Vector3i(R, L, M))) return Vector<2, Vector<3, T> >(ori_boundary_[1][0][0], ori_boundary_[1][0][1]);
    if (n == ToIndex(Vector3i(R, R, M))) return Vector<2, Vector<3, T> >(ori_boundary_[1][1][0], ori_boundary_[1][1][1]);
    if (n == ToIndex(Vector3i(L, R, M))) return Vector<2, Vector<3, T> >(ori_boundary_[0][1][0], ori_boundary_[0][1][1]);
    ZLOGF << "This coordinate is not a line" << ZVAR(pos);
    return Vector<2, Vector<3, T> >();
  }

  static bool IsPlane(const Vector3i &pos) {
    int n = ToIndex(pos);
    if (n == ToIndex(Vector3i(R, M, M)) ||
        n == ToIndex(Vector3i(L, M, M)) ||
        n == ToIndex(Vector3i(M, R, M)) ||
        n == ToIndex(Vector3i(M, L, M)) ||
        n == ToIndex(Vector3i(M, M, R)) ||
        n == ToIndex(Vector3i(M, M, L)))
      return true;
    return false;
  }

  Vector<4, Vector<3, T> > GetBoundaryPlane(const Vector3i& pos) const {
    if (!ori_boundary_good_)
      CalculateOriBoundary();
    int n = ToIndex(pos);
    if (n == ToIndex(Vector3i(R, M, M)))
      return Vector<4, Vector<3, T> >(
        ori_boundary_[1][0][1],
        ori_boundary_[1][0][0],
        ori_boundary_[1][1][0],
        ori_boundary_[1][1][1]);
    if (n == ToIndex(Vector3i(L, M, M)))
      return Vector<4, Vector<3, T> >(
        ori_boundary_[0][0][1],
        ori_boundary_[0][0][0],
        ori_boundary_[0][1][0],
        ori_boundary_[0][1][1]);
    if (n == ToIndex(Vector3i(M, R, M)))
      return Vector<4, Vector<3, T> >(
        ori_boundary_[0][1][0],
        ori_boundary_[1][1][0],
        ori_boundary_[1][1][1],
        ori_boundary_[0][1][1]);
    if (n == ToIndex(Vector3i(M, L, M)))
      return Vector<4, Vector<3, T> >(
        ori_boundary_[0][0][0],
        ori_boundary_[1][0][0],
        ori_boundary_[1][0][1],
        ori_boundary_[0][0][1]);
    if (n == ToIndex(Vector3i(M, M, R)))
      return Vector<4, Vector<3, T> >(
        ori_boundary_[0][0][1],
        ori_boundary_[1][0][1],
        ori_boundary_[1][1][1],
        ori_boundary_[0][1][1]);
    if (n == ToIndex(Vector3i(M, M, L)))
      return Vector<4, Vector<3, T> >(
        ori_boundary_[1][0][0],
        ori_boundary_[0][0][0],
        ori_boundary_[0][0][1],
        ori_boundary_[1][0][1]);
    ZLOGF << "This coordinate is not a plane" << ZVAR(pos);
    return Vector<4, Vector<3, T> >();
  }

  const Matrix<3, 3, T>& GetRot() const {return rot_;}
  const Matrix<3, 3, T>& GetRotBack() const {return rot_back_;}
  const AABB<3, T>& GetAABB() const {return aabb_;}
  Vector<3, T> Rotate(const Vector<3, T>& x) const {return rot_ * x;}
  Vector<3, T> RotateBack(const Vector<3, T>& x) const {return rot_back_ * x;}
private:
  AABB<3, T> aabb_;
  Matrix<3, 3, T> rot_;
  Matrix<3, 3, T> rot_back_;
  mutable Vector<3, T> ori_boundary_[2][2][2];
  mutable bool ori_boundary_good_;
  void CalculateOriBoundary() const {
    const Vector<3, T> &a = aabb_.Min();
    const Vector<3, T> &b = aabb_.Max();
    ori_boundary_[0][0][0] = rot_back_ * Vector<3, T>(a[0], a[1], a[2]);
    ori_boundary_[1][0][0] = rot_back_ * Vector<3, T>(b[0], a[1], a[2]);
    ori_boundary_[0][1][0] = rot_back_ * Vector<3, T>(a[0], b[1], a[2]);
    ori_boundary_[1][1][0] = rot_back_ * Vector<3, T>(b[0], b[1], a[2]);
    ori_boundary_[0][0][1] = rot_back_ * Vector<3, T>(a[0], a[1], b[2]);
    ori_boundary_[1][0][1] = rot_back_ * Vector<3, T>(b[0], a[1], b[2]);
    ori_boundary_[0][1][1] = rot_back_ * Vector<3, T>(a[0], b[1], b[2]);
    ori_boundary_[1][1][1] = rot_back_ * Vector<3, T>(b[0], b[1], b[2]);
    ori_boundary_good_ = true;
  }
};
typedef BoundingBox3<zfloat32> BoundingBox3f32;
typedef BoundingBox3<zfloat64> BoundingBox3f64;

SIMPLE_IOOBJECT(BoundingBox3f32);
SIMPLE_IOOBJECT(BoundingBox3f64);
}   // namespace zzz
